%author: wxj233
%time: 2023.11.12 10:00
%function:  特征点

classdef FeaturePoint < handle
    %FEATUREPOINT 特征点
    %   用于记录每一个轨迹的特征点
    
    properties
        id  % 特征点唯一标识
        points  % 特征点轨迹,其中points的结构为[t, x, vx, y, vy, id, cluster],id为真实目标id
        Xk_e  % 特征点当前估计值
        Pk_ee  % 特征点估计误差协方差
        lifeMax  % 特征点最大生命值
        isValidAss  % 是否为有效关联,每当与一个真实量测关联跟新该值赋值true，否则为false

        fplife  % 特征点生命值，从上次有效量测以来，每记忆一次减一，有效量测一次恢复最大生命值
        mNum  % 量测数量(measure Number),有效量测次数,每有效关联一次加一
        track  % 该特征点所属轨迹

        assP  % 用于状态跟新的关联点[t, x, y, id, cluster],没有则赋值为[]
    end
    
    methods
        function obj = FeaturePoint(lifeMax, points, Xk_e, Pk_ee, track)
            %FEATUREPOINT 构造此类的实例
            % lifeMax: 特征点最大生命值
            % points: 初始轨迹，长度为2
            % Xk_e: 初始状态
            % Pk_ee: 初始估计误差协方差矩阵
            % track: 当前特征点所属轨迹
            obj.id = obj.geId();
            obj.lifeMax = lifeMax;
            obj.points = points;
            obj.Xk_e = Xk_e;
            obj.Pk_ee = Pk_ee;
            obj.track = track;
            obj.assP = [];

            obj.isValidAss = true;
            obj.mNum = 2;
            obj.fplife = lifeMax;
        end

        function costs = costs(obj, points, inDoor, r)
            % 计算某些点属于该特征点轨迹的代价
            % points：待计算的点[t, x, y, id, cluster; ...]
            % r: 量测误差数量级
            % inDoor: 落入波门中的概率
            % return: 代价值，（这个地方是每个点的概率密度值，实际上是收益值）
            H = [1, 0, 0, 0;
                 0, 0, 1, 0];
            % 量测误差协方差矩阵
            R = [r, 0;
                 0, r];
            Xk_p = obj.Xk_e;  % 下一个状态预测值（因为状态已经换成了预测状态）
            Pk_pe = obj.Pk_ee;  % 预测误差协方差矩阵
            Zk_p = H*Xk_p;  % 量测预测

            Vks = points(:, [2,3])' - Zk_p;  % 新息（量测残差）
            Sk = H*Pk_pe*H'+R;  % 新息协方差

            costs = [];  % 代价矩阵,取距离为代价
            for i = 1:1:size(Vks, 2)
                Vk = Vks(:, i);
                chi = Vk'/Sk*Vk;  %  两个自由度的卡方分布
                p = 0;
                if chi < chi2inv(inDoor, 2)
                    p = 1/(2*pi*sqrt(det(Sk))) * exp(-0.5*chi);
                end
                costs = [costs, p];
            end
        end

        function [A, Q] = predict(obj, dt, q)
            % 状态预测
            % dt: 下一个时刻与当前时刻的时间差
            % q: 过程噪声数量级
            A = [1, dt, 0, 0;
                 0, 1, 0, 0;
                 0, 0, 1, dt;
                 0, 0, 0, 1];
            % 过程噪声
            Q = q*[0.1, 0, 0, 0;
                   0, 1, 0, 0;
                   0, 0, 0.1, 0;
                   0, 0, 0, 1];
            Xk_p = A*obj.Xk_e;  % 下一个状态预测值
            Pk_pe = A*obj.Pk_ee*A' + Q;  % 预测误差协方差矩阵

            obj.Xk_e = Xk_p;
            obj.Pk_ee = Pk_pe;
        end

        function [Kk, H, R, isExist] = update(obj, point, r, nowTime)
            % 状态更新,此方法会完成状态跟新，如果point为空则为记忆，若生命值为0则从track中删除该特征点
            % point: 用于状态更新的量测[t, x, y, id, cluster]
            % r: 量测误差方差
            % nowTime: 当前时刻

            % 量测矩阵
            H = [1, 0, 0, 0;
                 0, 0, 1, 0];
            % 量测误差协方差矩阵
            R = [r, 0;
                 0, r];

            isExist = true;
            if isempty(point)
                obj.fplife = obj.fplife - 1;
                obj.isValidAss = false;
                if obj.fplife == 0
                    isExist = false;
                    obj.track.deleteFp(obj.id);
                end
                obj.points = [obj.points; [nowTime, obj.Xk_e', 0, 0]];
                Kk = zeros([4,2]);
            else
                Zk = point(1, 2:3)';
                Xk_p = obj.Xk_e;  % 下一个状态预测值
                Pk_pe = obj.Pk_ee;  % 预测误差协方差矩阵
    
                Zk_p = H*Xk_p;  % 量测预测
                Vk = Zk - Zk_p;  % 新息（量测残差）
                Sk = H*Pk_pe*H'+R;  % 新息协方差
    
                Kk = Pk_pe*H'/Sk;  % 卡尔曼增益
                obj.Xk_e = Xk_p + Kk*Vk;
                obj.Pk_ee = Pk_pe - Kk*H*Pk_pe;

                obj.fplife = obj.lifeMax;
                obj.isValidAss = true;
                obj.mNum = obj.mNum + 1;

                obj.points = [obj.points; [nowTime, obj.Xk_e', point(1, 4:5)]];
            end
        end
    end

    methods (Static)
        function id = geId(varargin)
          % 获取特征点唯一标识
          persistent ID;
          if size(varargin, 2) == 1
            ID = varargin{1};
            return;
          end
          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end
end

